#include "MatlabDrawer.h"
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include "Util.h"
#include "MREAgent.h"

namespace matlab{


int MatlabDrawer::totalDrawers = 0; 

MatlabDrawer::MatlabDrawer(const char* n)
:MatlabInterface()
{
	totalDrawers++; 

	nRow = 1; 
	nCol = 1; 

	handleName = n; 
#ifdef MATLAB_PLOT
//	string cmd = handleName + " = figure('name', '" + handleName + "');";
	char* subCmd = new char[20]; 
	sprintf(subCmd, "subfig(2,2,%d);", totalDrawers); 
	string cmd = handleName + " = " + subCmd + " set(" + handleName + ", 'name', '" + handleName + "');"; 
	engEvalString(getEngine(), cmd.c_str() ); //create a new figure window 
#endif
}

MatlabDrawer::~MatlabDrawer(void)
{
}


/*
Given a matrix of data in the form of x1,x2,...,xn, value draws the value function using doPlot function in 
matlab (function doPlot is also written by me and should be available in the path of matlab). 
*/
void MatlabDrawer::drawMeshedValues(MatlabMatrix<double>& values, string type,  int pos)
{
#ifdef MATLAB_PLOT
	if (! getEngine())
		return; 

	mxArray* value_matrix = mxCreateDoubleMatrix(values.nRows(),values.nCols(), mxREAL); 
	double* start_ptr = (double*) mxGetPr(value_matrix); 
	memcpy(start_ptr, values.getStorage(), values.nRows()*values.nCols()*sizeof(double)); 

	int res = engPutVariable(getEngine(), "values", value_matrix); 


	//activate the designated window
	string cmd = "figure("; 
	cmd = cmd + handleName + ")"; 
	engEvalString(getEngine(), cmd.c_str()); 

	if (pos > 0)
	{
		char* tmp = new char[40]; 
		sprintf(tmp, "doPlot(values, '%s', %d, %d, %d);", type.c_str(), nRow, nCol, pos); 
		engEvalString(getEngine(), tmp); 
		delete[] tmp; 
	}else
	{
		cmd = "doPlot(values,'"; 
		cmd = cmd + type + "');"; 
		res = engEvalString(getEngine(), cmd.c_str()); 
	}


	mxDestroyArray(value_matrix); 
#endif
}


/*
Given the data of some boxes, draw them as grids in matlab. each row of the data is 
box_min1, box_max1, box_min2, box_max2, value (of that box). 

*/ 

void MatlabDrawer::drawGrids(MatlabMatrix<double>& values, int pos=0)
{
#ifdef MATLAB_PLOT
	if (!getEngine())
		return; 

	mxArray* plotData = mxCreateDoubleMatrix(values.nRows(), values.nCols(), mxREAL); 
	double* start_ptr = (double*) mxGetPr(plotData); 
	memcpy(start_ptr, values.getStorage(), values.nRows()*values.nCols()*sizeof(double)); 

	int res = engPutVariable(getEngine(), "plotData", plotData);
//	res = engEvalString(getEngine(), "plotData = plotData'"); 
//	res = engEvalString(getEngine(), "save plotData"); 

	//activate the designated window
	string cmd = "figure("; 
	cmd = cmd + handleName + ")"; 
	engEvalString(getEngine(), cmd.c_str()); 



	if (pos > 0)
	{
		char* tmpstr = new char[30]; 
		sprintf(tmpstr, "subplot(%d,%d,%d)", nRow, nCol, pos); 
		engEvalString(getEngine(), tmpstr);
		delete tmpstr; 
	}

	res = engEvalString(getEngine(), "for i=1:size(plotData,1), col = [ plotData(i,5), plotData(i,5), plotData(i,5)]; rectangle ('Position', [plotData(i,1), plotData(i,3), plotData(i,2) - plotData(i,1), plotData(i,4) - plotData(i,3)], 'EdgeColor', 'r', 'FaceColor', col ); end "); 
	 

	mxDestroyArray(plotData); 
#endif
}


/*
Given a matrix of data in the form of (N * 2) draws the points using scatter function in matlab 
*/
void MatlabDrawer::drawScatteredPoints(MatlabMatrix<double>& points,  int pos)
{
#ifdef MATLAB_PLOT
	if (! getEngine())
		return; 

	//activate the designated window
	string cmd = "figure("; 
	cmd = cmd + handleName + ")"; 
	engEvalString(getEngine(), cmd.c_str()); 

	mxArray* value_matrix = mxCreateDoubleMatrix(points.nRows(),points.nCols(), mxREAL); 
	double* start_ptr = (double*) mxGetPr(value_matrix); 
	memcpy(start_ptr, points.getStorage(), points.nRows()*points.nCols()*sizeof(double)); 

	int res = engPutVariable(getEngine(), "values", value_matrix); 


	//transpose if necessary 
	if (points.nRows()< points.nCols())
		engEvalString(getEngine(), "values = values';"); 



	if (pos > 0)
	{
		char* tmpstr = new char[30]; 
		sprintf(tmpstr, "subplot(%d,%d,%d)", nRow, nCol, pos); 
		engEvalString(getEngine(), tmpstr);
		delete tmpstr; 
	}

	res = engEvalString(getEngine(), "scatter(values(:,1), values(:,2),'.');"); 
	mxDestroyArray(value_matrix); 
#endif
}

/* the person who calls this function should delete the matrix after being used */

MatlabMatrix<double>& MatlabDrawer::createMeshedDataStructure(const int resolution, int howMany, MREAgent *agent)
{
	const int rows = resolution * resolution; 
	const int cols = 2 + howMany; //this is always 3 (x,y and value)

	int result = 0; 

	MatlabMatrix<double>* valPtr = new MatlabMatrix<double>(rows, cols); 
	MatlabMatrix<double>& vals = *valPtr; 

	int cntr = 0; 
	for (int i=0; i< resolution; i++)
	{
		double first =	agent->taskSpec.double_observations[0].min  + i*(agent->taskSpec.double_observations[0].max  - agent->taskSpec.double_observations[0].min )/(resolution -1); 
		if (first == agent->taskSpec.double_observations[0].max  ) 
			first -= 0.001; 
		else if (first == agent->taskSpec.double_observations[0].min)
			first += 0.001; 

		for(int j=0; j< resolution; j++, cntr++)
		{
			vals(cntr,0) = first; 

			vals(cntr,1) =	agent->taskSpec.double_observations[1].min + j*(agent->taskSpec.double_observations[1].max - agent->taskSpec.double_observations[1].min)/(resolution -1); 
			if (vals(cntr,1) == agent->taskSpec.double_observations[1].max ) 
				vals(cntr,1) -= 0.0001; 
			else if (vals(cntr,1) == agent->taskSpec.double_observations[1].min)
				vals(cntr,1) += 0.0001; 

			for(int k=0; k < howMany; k++) vals(cntr,2+k) = 0;			//default value
		}
	}

	return vals; 
}
}
